#!/usr/bin/env python
import roslib; roslib.load_manifest('smc_04b')
import rospy
import serial
from std_msgs.msg import String

class odometryPublisher:

	

	def __init__(self):	# Initialization method
		self.a = serial.Serial('/dev/robot/arduino', 9600)
		self.PI	     = 3.141529
		self.WHEEL_RADIUS = 3*(2.54)/100 # 3 inches in meters
		self.AXIS_DIST    = 23.5*(2.54)/100 # 23.5 inches in meters


	def publish_odom(self): 	# Publish robot velocity, vel, to cmd_vel (yes?) 
		rospy.init_node('publish_odom')
		
		while not rospy.is_shutdown():
			
			blipPS = self.a.readline();	### Get 'xxx yyy zzzzzz\r\n'
			blipPS = blipPS.strip('\r\n')   ### Get 'xxx yyy zzzzzz'
			blipPS = blipPS.split(' ') 	### Get 'xxx','yyy','zzzzzz'

			try: 
				blips_m1 = int(blipPS[0])
			except ValueError:
				blips_m1 = 0;
				print "non-printable text read: forcing blips_m1 to 0"	
	
			try: 
				blips_m2 = int(blipPS[1])
			except ValueError:
				blips_m2 = 0;
				print "non-printable text read: forcing blips_m2 to 0"
			except IndexError:
				blips_m2 = 0;
				print "Non-printable text: forcing blips_m2 to 0"	

			try: 
				d_time = int(blipPS[2])
			except ValueError:
				d_time = 50000
				print "non-printable text read: forcing d_time to 50000"
			except IndexError:
				d_time = 50000
				print "Non-printable text, d_time is now 50000"	

			self.blips_to_mps(blips_m1, blips_m2, d_time)
			
	def blips_to_mps(self, blips_m1, blips_m2, d_time):
		#print str(blips_m1) + " " + str(blips_m2) + " " + str(d_time)
	
		# Have number of blips turned in zzz zzz microseconds - lowest speed setting is ~150 blips per 500,000 microseconds (.5 seconds) 
				# want to calc ang wheel velocities for each wheel
		# How many blips in a rotation of the wheel?  
		# - Motor says 500 cpr, w/ gear ratio of 19.7:1. So, 500*19.7 = 9850 blips per rotation, bpr

		### xxx blips       1,000,000 mu.s     1 rotation 	2*pi radians
		### ------------- * -------------- * --------------- * -------------- ->
		### zzz,zzz mu.s      1 second		9850 blips	  1 rotation

		### ==>  ang_vel = (xxx blips/zzzzzz mu.s)*(101.5228)*2*pi radians per second

		### This would give speed of each wheel:  There are 20,570 blips in a meter. Elapsed_time is in microseconds
		#	  xxx blips	       1 meter         1000000 mu.s
		#	----------------- * -------------- * ------------ = Xlin/elapsed_time m/s ==> 
		#	zzzzzz (mu.s)        20573 blips      1 second
		


		# angular velocity of 1st motor wheel (right motor):
		omega_r = (blips_m1*101.5228*2*3.141529)/(d_time)

		# angular velocity of 2nd motor wheel (left motor):
		omega_l = (blips_m2*101.5228*2*3.141529)/(d_time)	
	
		# Then, calc vel_lin and vel_ang for whole robot, given omegR and omegL of the wheels
		# Rr = 3 inches = Rl
		# D = 23.5 or something
		# vel.linear = (omega_r * Rr + omega_l * Rl) / 2
		# vel.angular = (omega_r * Rr - omega_l * Rl) / D

		# Calc linear velocity of whole system
		vel_lin = (omega_l + omega_r)*self.WHEEL_RADIUS/2

		# Calc angular velocity of whole system
		vel_ang = (omega_l + omega_r)*self.WHEEL_RADIUS/self.AXIS_DIST
		
		print "Lin vel is " + str(vel_lin) + ". Ang vel is " + str(vel_ang)
	
		# Now, calc odometry info: this is generally copied from odom tutorial from robot setup under navigation
		x = 0.0
		y = 0.0
		th = 0.0
		
		vx = 0.0
		vy = 0.0
		vth = 0.0
	 
		# Then:
		# dx = vel.lin*cos(theta) * dt
		# dy = vel.lin*sin(theta) * dt
		# dtheta = omega * dt

if __name__ == '__main__':
	op = odometryPublisher()
	print "Creating new instance of odomPub"
	op.publish_odom()
	print "Beginning publishing motor vels"
